
#include "main.h"


static LINEEQ_DATA _LineEqData[CAMERA_MAX_NUM][LINEEQ_DATA_MAX_NUM] = {0,};
static BYTE _bLineEqData[CAMERA_MAX_NUM] = {0,};
static BYTE _bTouchProc = 0;


void Touch_Init(void)
{
}
void Touch_Proc(void)
{
	if (0 == _bTouchProc)
		return;
	_bTouchProc = 0;

	POINT_DATA newPoints[POINT_DATA_MAX_NUM] = {0,};
	u8 nNewPoint = 0;
	for (int l=0; l<min(_bLineEqData[CAMERA_LEFT], TOUCH_DATA_MAX_NUM); l++) {
		for (int r=0; r<min(_bLineEqData[CAMERA_RIGHT], TOUCH_DATA_MAX_NUM); r++) {
			if (POINT_DATA_MAX_NUM <= nNewPoint) {
				break;
			}
			point_intersection(&_LineEqData[CAMERA_LEFT][l], &_LineEqData[CAMERA_RIGHT][r], &newPoints[nNewPoint++]);
		}
	}

	static u8 _nDownTouch = 0, _nOldPoint = 0;
	static TOUCH_DATA _downTouchs[TOUCH_DATA_MAX_NUM] = {0,};
	float fDist0, fDist1;
	u8 bIdx, bDnIdx;
	const float fREJECT_DISTANCE = (float)0.05;	// 5 [%]

	if (g_TCD.bTouchNoSend) {
		_nDownTouch = _nOldPoint = 0;
		return;
	}

	switch (nNewPoint) {
	case 0:	// all touch up
		for (int i=0; i<TOUCH_DATA_MAX_NUM; i++) {
			touch_up(&_downTouchs[i]);
		}
		break;
	case 1:
		switch (_nDownTouch) {
		case 0:	// 0->1
			touch_new(&_downTouchs[0], &newPoints[0], TOUCH_ID_0);
			break;
		case 1:	// 1->1
			touch_dn(&_downTouchs[0], &newPoints[0]);
			break;
		case 2:	// 2->1
			fDist0 = point_distance(&_downTouchs[0].fPt, &newPoints[0].fPt);
			fDist1 = point_distance(&_downTouchs[1].fPt, &newPoints[0].fPt);
			bIdx = (fDist0 < fDist1) ? 1 : 0;
			touch_up(&_downTouchs[bIdx]);

			bIdx = bIdx ? 0 : 1;
			fDist1 = (fDist0 < fDist1) ? fDist1 : fDist0;
			if (fREJECT_DISTANCE < fDist1) {
				printf("1=>%f\r\n", fDist1);
				touch_up(&_downTouchs[bIdx]);
				nNewPoint = 0;
			} else {
				touch_dn(&_downTouchs[bIdx], &newPoints[0]);
			}
			break;
		}
		break;
	case 2:
		switch (_nDownTouch) {
		case 0:	// 0->2
			touch_new(&_downTouchs[0], &newPoints[0], TOUCH_ID_0);
			touch_new(&_downTouchs[1], &newPoints[1], TOUCH_ID_1);
			break;
		case 1:	// 1->2
			fDist0 = point_distance(&_downTouchs[0].fPt, &newPoints[0].fPt);
			fDist1 = point_distance(&_downTouchs[0].fPt, &newPoints[1].fPt);
			bIdx = (fDist0 < fDist1) ? 0 : 1;
			touch_dn(&_downTouchs[0], &newPoints[bIdx]);

			bIdx = bIdx ? 0 : 1;
			touch_new(&_downTouchs[1], &newPoints[bIdx], (TOUCH_ID_0==_downTouchs[0].bId) ? TOUCH_ID_1 : TOUCH_ID_0);
			break;
		case 2:	// 2->2
			// 4->2, 2->2
			if (4 == _nOldPoint) {
				fDist1 = 1;
				for (int i=0, j; i<_nDownTouch; i++) {
					for (j=0; j<nNewPoint; j++) {
						fDist0 = point_distance(&_downTouchs[i].fPt, &newPoints[j].fPt);
						if (fDist1 > fDist0) {
							bDnIdx = i;
							bIdx = j;
							fDist1 = fDist0;
						}
					}
				}
				if (fREJECT_DISTANCE < fDist1) {
					printf("20=>%f\r\n", fDist1);
					touch_up(&_downTouchs[bDnIdx]);
					nNewPoint--;
				} else {
					touch_dn(&_downTouchs[bDnIdx], &newPoints[bIdx]);
				}

				bDnIdx = (0==bDnIdx) ? 1 : 0;
				bIdx = (0==bIdx) ? 1 : 0;
				fDist1 = point_distance(&_downTouchs[bDnIdx].fPt, &newPoints[bIdx].fPt);
				if (fREJECT_DISTANCE < fDist1) {
					printf("21=>%f\r\n", fDist1);
					touch_up(&_downTouchs[bDnIdx]);
					nNewPoint--;
				} else {
					touch_dn(&_downTouchs[bDnIdx], &newPoints[bIdx]);
				}
			} else /*if (2 == _nOldPoint)*/ {
				bDnIdx = (_downTouchs[0].uCount >= _downTouchs[1].uCount) ? 0 : 1;
				fDist1 = 1;
				for (int i=0; i<nNewPoint; i++) {
					fDist0 = point_distance(&_downTouchs[bDnIdx].fPt, &newPoints[i].fPt);
					if (fDist1 > fDist0) {
						bIdx = i;
						fDist1 = fDist0;
					}
				}
				if (fREJECT_DISTANCE < fDist1) {
					printf("22=>%f\r\n", fDist1);
					touch_up(&_downTouchs[bDnIdx]);
					nNewPoint--;
				} else {
					touch_dn(&_downTouchs[bDnIdx], &newPoints[bIdx]);
				}

				bDnIdx = (0==bDnIdx) ? 1 : 0;
				bIdx = (0==bIdx) ? 1 : 0;
				fDist1 = point_distance(&_downTouchs[bDnIdx].fPt, &newPoints[bIdx].fPt);
				if (fREJECT_DISTANCE < fDist1) {
					printf("23=>%f\r\n", fDist1);
					touch_up(&_downTouchs[bDnIdx]);
					nNewPoint--;
				} else {
					touch_dn(&_downTouchs[bDnIdx], &newPoints[bIdx]);
				}
			}
			break;
		}
		break;
	case 4:
		switch (_nDownTouch) {
		case 0:	// 0->4 new
			// TODO : ghost
			printf("ghost\r\n");
			touch_new(&_downTouchs[0], &newPoints[0], TOUCH_ID_0);
			touch_new(&_downTouchs[1], &newPoints[3], TOUCH_ID_1);
			break;
		case 1:	// 1->4 new
			fDist1 = 1;
			for (int i=0; i<nNewPoint; i++) {
				if (newPoints[i].bIsValid) {
					fDist0 = point_distance(&_downTouchs[0].fPt, &newPoints[i].fPt);
					if (fDist1 > fDist0) {
						bIdx = i;
						fDist1 = fDist0;
					}
				}
			}
			touch_dn(&_downTouchs[0], &newPoints[bIdx]);

			bIdx = (0==bIdx) ? 3 : (1==bIdx) ? 2 : (2==bIdx) ? 1 : 0;
			touch_new(&_downTouchs[1], &newPoints[bIdx], (TOUCH_ID_0==_downTouchs[0].bId) ? TOUCH_ID_1 : TOUCH_ID_0);
			break;
		case 2:	// 2->4 find
			bDnIdx = (_downTouchs[0].uCount >= _downTouchs[1].uCount) ? 0 : 1;
			fDist1 = 1;
			for (int i=0; i<nNewPoint; i++) {
				fDist0 = point_distance(&_downTouchs[bDnIdx].fPt, &newPoints[i].fPt);
				if (fDist1 > fDist0) {
					bIdx = i;
					fDist1 = fDist0;
				}
			}
			if (((1 == bIdx) || (2 == bIdx)) && (0 == newPoints[1].bIsValid)) {
				printf("40=>ghost\r\n");
				touch_up(&_downTouchs[0]);
				touch_up(&_downTouchs[1]);
				nNewPoint = 0;
			} else {
				touch_dn(&_downTouchs[bDnIdx], &newPoints[bIdx]);

				bDnIdx = (0==bDnIdx) ? 1 : 0;
				bIdx = (0==bIdx) ? 3 : (1==bIdx) ? 2 : (2==bIdx) ? 1 : 0;
				fDist1 = point_distance(&_downTouchs[bDnIdx].fPt, &newPoints[bIdx].fPt);
				if (fREJECT_DISTANCE < fDist1) {
					printf("41=>%f\r\n", fDist1);
					touch_up(&_downTouchs[bDnIdx]);
					nNewPoint = 1;
				} else {
					touch_dn(&_downTouchs[bDnIdx], &newPoints[bIdx]);
				}
			}
			break;
		}
		break;
	}

	_nDownTouch = touch_send(_downTouchs, min(nNewPoint, TOUCH_DATA_MAX_NUM), _nDownTouch);
	_nOldPoint = nNewPoint;
}
void Touch_ChangeState(u8 state)
{
	_bTouchProc = 0;
	//memset(_LineEqData, 0, sizeof(_LineEqData));
	memset(_bLineEqData, 0, sizeof(_bLineEqData));
}

void Touch_AddData(u8 dev, u16 dlen, u8 *pdata)
{
	u8 bIdx = dev - DEV_CAMERA1;

	if (CAMERA_MAX_NUM > bIdx) {
		int nCount = dlen / sizeof(LINEEQ_DATA), nDelay;

		if (LINEEQ_DATA_MAX_NUM >= nCount) {
			static DWORD _dwRecvTime[CAMERA_MAX_NUM] = {0,};

			if (nCount) {
				memcpy(&_LineEqData[bIdx][0], pdata, dlen);
			}
			_bLineEqData[bIdx] = nCount;
			_dwRecvTime[bIdx] = g_vu32SysTickCount;

			nDelay = (_dwRecvTime[0]>_dwRecvTime[1]) ? (_dwRecvTime[0]-_dwRecvTime[1]) : (_dwRecvTime[1]-_dwRecvTime[0]);
			if (8 > nDelay)	{	// < 8 msec
				_bTouchProc = 1;
			}
		}
	}
}

